/**
 * @file image2pcd.cpp
 * @brief Image to Point Cloud Data Conversion Tool
 *
 * This program implements the functionality of generating point cloud data from semantic segmentation images,
 * primarily used for autonomous driving mapping and localization scenarios.
 * The program can read image sequences and vehicle pose data, convert 2D semantic images to 3D point clouds,
 * and provide real-time visualization capabilities.
 *
 * Main Features:
 * - Semantic image to point cloud data conversion
 * - Vehicle pose data processing
 * - Point cloud clustering and filtering
 * - Real-time visualization display
 * - PCD/TXT format conversion
 */

#include <iostream>
#include <opencv2/opencv.hpp>

#include <pcl/filters/approximate_voxel_grid.h>
#include "pcl/common/common.h"
#include "pcl/common/transforms.h"
#include "pcl/filters/crop_box.h"
#include "pcl/io/pcd_io.h"
#include "pcl/registration/icp.h"
#include "pcl/registration/ndt.h"

#include "pcl/filters/radius_outlier_removal.h"

#include "pcl/visualization/pcl_visualizer.h"

#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>

#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h> // Line fitting

#include <thread>

#include <termios.h>
#include <unistd.h>
#include <cstdlib>

#include <sys/stat.h>
#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace cv;

// Global variables
int g_save_rotated_pcd = 0; // Whether to save rotated point cloud
int g_save_semantic_01 = 0; // Whether to save parking line semantics
int g_mapping_mode = 0;		// Whether in mapping mode
int g_jpg_size = 0;			// Number of JPG images

int g_jpg_count = 0; // JPG counter

// Vehicle dimension parameters
// float g_car_length = 4.701;
// float g_car_width = 1.92;
// float g_back_to_rear = 1.2827;

float g_car_length = 4.8;							   // Vehicle length
float g_car_width = 2.0;							   // Vehicle width
float g_back_to_rear = 1.3;							   // Distance from rear axle to back
float g_front_to_rear = g_car_length - g_back_to_rear; // Distance from front axle to back

// Point coordinate variables
float point1_x, point1_y, point2_x, point2_y, point3_x, point3_y, point4_x, point4_y;
float point1_x_new, point1_y_new, point2_x_new, point2_y_new, point3_x_new, point3_y_new, point4_x_new, point4_y_new;
Eigen::VectorXf line_coefficients1; // First fitted line coefficients
Eigen::VectorXf line_coefficients2; // Second fitted line coefficients
bool is_paused = false;				// Pause flag
float angle1 = 0;					// Angle 1
float angle2 = 0;					// Angle 2

// Visualization component initialization
std::shared_ptr<std::thread> process_thread_;
pcl::visualization::PCLVisualizer viewer;

/**
 * @brief Timer class for measuring code execution time
 */
class TicToc {
public:
	TicToc() { tic(); }

	void tic() { start = std::chrono::high_resolution_clock::now(); }

	double toc() {
		end = std::chrono::high_resolution_clock::now();
		std::chrono::milliseconds elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
		start = std::chrono::high_resolution_clock::now();
		return elapsed_seconds.count();
	}

private:
	std::chrono::high_resolution_clock::time_point start, end;
};

/**
 * @brief Vehicle pose structure
 */
struct CarPose {
	CarPose() : x(0.f), y(0.f), z(0.f), yaw(0.f), pitch(0.f), roll(0.f), timestamp(0.f) {}
	CarPose(float a, float b, float c, int64_t t) : x(a), y(b), z(0.f), yaw(c), pitch(0.f), roll(0.f), timestamp(t) {}
	CarPose(float a, float b, float c, float d, float e, float f, int64_t t)
		: x(a), y(b), z(c), yaw(d), pitch(e), roll(f), timestamp(t) {}
	friend std::ostream &operator<<(std::ostream &os, const CarPose &p) {
		os << "(" << p.x << "," << p.y << "," << p.z << "," << p.yaw << "," << p.pitch << "," << p.roll << ","
		   << p.timestamp << ")";
		return os;
	}
	CarPose &operator=(const CarPose &rhs) {
		x = rhs.x;
		y = rhs.y;
		z = rhs.z;
		yaw = rhs.yaw;
		pitch = rhs.pitch;
		roll = rhs.roll;
		timestamp = rhs.timestamp;
		return *this;
	}

	float x;		   // X coordinate
	float y;		   // Y coordinate
	float z;		   // Z coordinate
	float yaw;		   // Yaw angle
	float pitch;	   // Pitch angle
	float roll;		   // Roll angle
	int64_t timestamp; // Timestamp
};

/**
 * @brief Check if file exists
 * @param pszFilePath File path
 * @return true if exists, false otherwise
 */
bool IsFileExist(const char *pszFilePath) {
#ifdef OS_WIN
	return _access(pszFilePath, _A_NORMAL) != -1;
#else
	return access(pszFilePath, F_OK) != -1;
#endif
}

/**
 * @brief Execute command and get result
 * @param strCmd Command to execute
 * @return Command execution result string
 */
std::string getCmdResult(const std::string &strCmd) {
	char buf[1024] = { 0 };
	FILE *pf = NULL;
	if ((pf = popen(strCmd.c_str(), "r")) == NULL) {
		return "";
	}
	std::string strResult;
	while (fgets(buf, sizeof buf, pf)) {
		strResult += buf;
	}
	pclose(pf);
	unsigned int iSize = strResult.size();
	if (iSize > 0 && strResult[iSize - 1] == '\n') {
		// Remove newline character on Linux systems
		strResult = strResult.substr(0, iSize - 1);
	}
	return strResult;
}

/**
 * @brief String split function
 * @param s String to split
 * @param v Vector to store split results
 * @param c Delimiter
 */
void SplitString(const std::string &s, std::vector<std::string> &v, const std::string &c) {
	std::string::size_type pos1, pos2;
	pos2 = s.find(c);
	pos1 = 0;
	while (std::string::npos != pos2) {
		v.push_back(s.substr(pos1, pos2 - pos1));

		pos1 = pos2 + c.size();
		pos2 = s.find(c, pos1);
	}
	if (pos1 != s.length()) {
		v.push_back(s.substr(pos1));
	}
	// Debug output
	// for (size_t i = 0; i < v.size(); i++) {
	//   std::cout << "v i " << v[i] << std::endl;
	// }
}

/**
 * @brief Convert top-view image coordinates to vehicle coordinate system
 * @param topviewpoint Top-view coordinate point
 * @param scale_x X-axis scale factor
 * @param scale_y Y-axis scale factor
 * @return Point in vehicle coordinate system
 */
Eigen::Vector2f TopViewPoint_to_SelfPoint(Eigen::Vector2f &topviewpoint, float scale_x, float scale_y) {
	Eigen::Vector2f car_center(400.f, 475.0f);
	Eigen::Vector2f selfpoint;
	// Coordinate system conversion: up x, left y
	selfpoint[0] = -topviewpoint[1] + car_center[1];
	selfpoint[1] = -topviewpoint[0] + car_center[0];

	// Alternative coordinate system conversion (right x, up y) - This vehicle coordinate system is most consistent with real path display
	// selfpoint[0] = topviewpoint[0] - car_center[0];
	// selfpoint[1] = -topviewpoint[1] + car_center[1];

	// Alternative coordinate system conversion (right x, down y)
	// selfpoint[0] = topviewpoint[0] - car_center[0];
	// selfpoint[1] = topviewpoint[1] - car_center[1];

	selfpoint[0] = selfpoint[0] * scale_x;
	selfpoint[1] = selfpoint[1] * scale_y;
	return selfpoint;
}

/**
 * @brief Data classification function, determines whether to retain data based on label
 * @param l Data label
 * @return true to retain, false to discard
 *
 * Data label description:
 * 0 - Parking lines      1 - Lane lines        2 - Lane center lines
 * 3 - Direction arrows   4 - Crosswalks        5 - No parking signs
 * 6 - Speed bumps        7 - Pillars           8 - Vehicles
 * 9 - Limiters          10 - Walls            11 - Ground
 * 12 - Roadside rocks
 */
bool ClassifyData(int l) {
	// 0 Parking lines
	// 1 Lane lines
	// 2 Lane center lines
	// 3 Direction arrows
	// 4 Crosswalks
	// 5 No parking signs
	// 6 Speed bumps
	// 7 Pillars
	// 8 Vehicles
	// 9 Limiters
	// 10 Walls
	// 11 Ground
	// 12 Roadside rocks

	if (g_mapping_mode == 0) {
		// Tracking mode
		//  if (l > 0) {
		//      return false;
		//  }

		if (l == 7) {
			return false;
		}

		if (l > 9) {
			return false;
		}

		switch (l) {
		case 0:
		case 1:
		case 8:
		case 9:
			// case 9:
			return true;
		default:
			return false;
		}
	}
	else {
		// Mapping mode
		if (l > 6) {
			return false;
		}
	}

	// Parking line and lane line semantic filtering
	if (g_save_semantic_01 == 0) {
		if (l == 0 || l == 1) {
			return false;
		}
	}
	return true;
}

/**
 * @brief Single point cloud clustering function
 * @param cloud Input point cloud
 * @param clouds_out Output clustered point cloud
 * @return true on success, false on failure
 */
bool ClusterSingle(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr &clouds_out) {
	int min_size_ = 5;				 // Minimum cluster size
	int max_size_ = 5000;			 // Maximum cluster size
	float cluster_tolerance_ = 0.25; // Cluster tolerance
	pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec_;
	std::vector<pcl::PointIndices> cluster_indices_;
	// std::vector<typename pcl::PointCloud<pcl::PointXYZI>::Ptr> clusters;

	// Perform Euclidean clustering to group detected obstacles
	pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
	tree->setInputCloud(cloud);

	ec_.setClusterTolerance(cluster_tolerance_);
	ec_.setMinClusterSize(min_size_);
	ec_.setMaxClusterSize(max_size_);
	ec_.setSearchMethod(tree);
	ec_.setInputCloud(cloud);
	ec_.extract(cluster_indices_);

	for (auto &getIndices : cluster_indices_) {
		pcl::PointCloud<pcl::PointXYZI>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZI>);

		for (auto &index : getIndices.indices) {
			clouds_out->points.push_back(cloud->points[index]);
		}

		clouds_out->width = clouds_out->points.size();
		clouds_out->height = 1;
		clouds_out->is_dense = true;

		// clusters.push_back(cluster);
		// std::cout << "clouds_out " << clouds_out->points.size() << std::endl;
		// clouds_out.push_back(cluster);
		// cluster->clear();
	}

	cluster_indices_.clear();

	// std::cout << "cloud input " << cloud->points.size() << std::endl;
	// std::cout << "Current point cloud cluster count: " << clouds_out->width << std::endl;

	return true;
}

/**
 * @brief Convert image to point cloud data
 * @param jpgPath Image path
 * @param pcl_out Output point cloud
 * @param odom_guess Pose transformation matrix
 * @param bWhole Whether to process the entire image
 */
void jpg_to_pcl(std::string jpgPath, pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_out, Eigen::Matrix4d odom_guess,
	bool bWhole = false) {
	int image_height = 800;		// Image height
	int image_width = 800;		// Image width
	float pixel_scale_x = 0.02; // X-axis pixel scale
	float pixel_scale_y = 0.02; // Y-axis pixel scale

	int center_x = image_width / 2;
	int center_y = image_height / 2;

	TicToc timeTT;
	cv::Mat mask = imread(jpgPath, CV_8UC1);

	Eigen::Affine3f aff_pose;

	aff_pose.matrix() = odom_guess.cast<float>();
	float x, y, z, roll, pitch, yaw;
	pcl::getTranslationAndEulerAngles(aff_pose, x, y, z, roll, pitch, yaw);

	pcl::PointCloud<pcl::PointXYZI>::Ptr out_filter_cloud(new pcl::PointCloud<pcl::PointXYZI>);
	// Boundary filtering preprocessing
	// for (int r = 0; r < image_height; ++r) {
	//     for (int l = 0; l < image_width; ++l) {
	//         if (r < up || r > down || l < left || l > right) {
	//             mask.at<uint8_t>(r, l) = 11;
	//         }
	//     }
	// }
	// std::cout << "for 1 read cost ms " << timeTT.toc() << std::endl;
	pcl_out->clear();

	pcl::PointCloud<pcl::PointXYZI>::Ptr pillar_cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr pillar_cloud_cluster(new pcl::PointCloud<pcl::PointXYZI>);
	for (int angle = 0; angle < 360; angle += 2) {
		// Search for pixels with the same RGB value along the ray
		for (int i = 0; i < image_width / 2; i++) {
			int x_i = center_x + std::cos(angle * CV_PI / 180.0) * i;
			int y_i = center_y + std::sin(angle * CV_PI / 180.0) * i;

			// 7 Pillar 10 Wall
			// bool bCar = value == 8 && (tmp_point.x > 5 || tmp_point.x < -3 || fabs(tmp_point.y) > 1.5)
			int value = mask.at<uint8_t>(y_i, x_i);
			// if (value == 7 || value == 10 || value == 8) {
			if (value == 8) {
				// Store the nearest point
				Eigen::Vector2f img_point(static_cast<float>(x_i), static_cast<float>(y_i));
				Eigen::Vector2f self_point = TopViewPoint_to_SelfPoint(img_point, pixel_scale_x, pixel_scale_y);
				pcl::PointXYZI tmp_point;
				tmp_point.x = self_point[0];
				tmp_point.y = self_point[1];
				tmp_point.intensity = value;
				if (value == 8 && (tmp_point.x < g_front_to_rear && tmp_point.x > -g_back_to_rear &&
									  fabs(tmp_point.y) < g_car_width / 2.0)) {
					// std::cout << "x " << tmp_point.x << " tmp_point.y " << tmp_point.y << std::endl;
					continue;
				}
				pillar_cloud->push_back(tmp_point);
				break;
			}
		}
	}

	ClusterSingle(pillar_cloud, pillar_cloud_cluster);

	cv::Mat outputImage(mask.rows, mask.cols, CV_8UC3, cv::Scalar(0, 0, 0));
	// Parallel pixel processing
	// #pragma omp parallel for
	for (int i = 0; i < mask.rows; i += 2) {
		for (int j = 0; j < mask.cols; j += 2) {
			int value = int(mask.at<uint8_t>(i, j));

			switch (value) {

			case 0:
				outputImage.at<Vec3b>(i, j)[0] = 255;
				outputImage.at<Vec3b>(i, j)[1] = 0;
				outputImage.at<Vec3b>(i, j)[2] = 0;
				break;
			case 1:
				outputImage.at<Vec3b>(i, j)[0] = 0;
				outputImage.at<Vec3b>(i, j)[1] = 0;
				outputImage.at<Vec3b>(i, j)[2] = 255;
				break;
			case 2:
				outputImage.at<Vec3b>(i, j)[0] = 0;
				outputImage.at<Vec3b>(i, j)[1] = 255;
				outputImage.at<Vec3b>(i, j)[2] = 0;
				break;
			case 3:
				outputImage.at<Vec3b>(i, j)[0] = 255;
				outputImage.at<Vec3b>(i, j)[1] = 255;
				outputImage.at<Vec3b>(i, j)[2] = 0;
				break;
			case 4:
				outputImage.at<Vec3b>(i, j)[0] = 0;
				outputImage.at<Vec3b>(i, j)[1] = 255;
				outputImage.at<Vec3b>(i, j)[2] = 255;
				break;
			case 5:
				outputImage.at<Vec3b>(i, j)[0] = 255;
				outputImage.at<Vec3b>(i, j)[1] = 0;
				outputImage.at<Vec3b>(i, j)[2] = 255;
				break;
			case 6:
				outputImage.at<Vec3b>(i, j)[0] = 255;
				outputImage.at<Vec3b>(i, j)[1] = 255;
				outputImage.at<Vec3b>(i, j)[2] = 255;
				break;
			}
			if (ClassifyData(value)) {
				Eigen::Vector2f img_point(static_cast<float>(j), static_cast<float>(i));
				Eigen::Vector2f self_point = TopViewPoint_to_SelfPoint(img_point, pixel_scale_x, pixel_scale_y);
				pcl::PointXYZI tmp_point;
				tmp_point.x = self_point[0];

				if (g_mapping_mode == 1) {
					// Mapping mode range restriction
					if (bWhole == false) {
						if (tmp_point.x > 4 || tmp_point.x < -2) {
							continue;
						}
					}
				}

				tmp_point.y = self_point[1];
				tmp_point.intensity = value;

				if (value == 8 && (tmp_point.x > g_front_to_rear || tmp_point.x < -g_back_to_rear ||
									  fabs(tmp_point.y) > g_car_width / 2.0)) {
					// cout << "value " << value << std::endl;
					// continue;
				}

				pcl_out->push_back(tmp_point);
			}
		}
	}

	// Set and save point cloud
	pcl_out->height = 1;
	pcl_out->width = pcl_out->points.size();
	pcl_out->is_dense = false;

	// std::cout << "for 2 jpg cost ms " << timeTT.toc() << std::endl;

	std::string pcdPath = "";
	// std::string pcdFilterPath = "";

	std::string segImagePath = "";

	std::vector<std::string> vecTemp;
	vecTemp.clear();
	SplitString(jpgPath, vecTemp, ".");

	pcdPath = vecTemp[0] + ".pcd";

	segImagePath = vecTemp[0] + ".jpg";

	cout << "-------------segImagePath " << segImagePath << endl;

	imwrite(segImagePath.c_str(), outputImage);

	if (pcl_out->points.size() < 20) {
		return;
	}

	// Downsampling processing
	if (pcl_out->points.size() > 3000) {
		float voxel_leaf_size_ = 0.05;
		pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter_scan;
		voxel_grid_filter_scan.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
		voxel_grid_filter_scan.setInputCloud(pcl_out);
		voxel_grid_filter_scan.filter(*pcl_out);

		// std::cout << "pcd voxel_grid_filter_scan "
		//           << " pcl_out->size() " << pcl_out->size() << std::endl;
	}

	// std::cout << "for 3 voxel cost ms " << timeTT.toc() << std::endl;

	// Radius filtering (optional)
	// pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_map_filter(new pcl::PointCloud<pcl::PointXYZI>);
	// pcl::RadiusOutlierRemoval<pcl::PointXYZI> ror;
	// ror.setInputCloud(pcl_out);       // Set input point cloud
	// ror.setRadiusSearch(0.5);         // Set search radius
	// ror.setMinNeighborsInRadius(50);  // Set minimum number of neighbors within radius
	// ror.filter(*pcl_out);             // Execute filtering operation

	*pcl_out += *pillar_cloud_cluster;

	// std::cout << "for 4 radius cost ms " << timeTT.toc() << std::endl;

	pcl::transformPointCloud(*pcl_out, *pcl_out, odom_guess);

	// std::cout << "pcd path: " << pcdPath << " pcl_out->size() " << pcl_out->size() << std::endl;
	// std::cout << "cloud_map_filter path: " << pcdPath << " cloud_map_filter->size() " << cloud_map_filter->size()
	//           << std::endl;

	if (pcl_out->points.size() < 5) {
		return;
	}

	std::cout << "pcdPath " << pcdPath.c_str() << std::endl;
	pcl::io::savePCDFileBinary(pcdPath.c_str(), *pcl_out);
	// std::cout << "for 5 save cost ms " << timeTT.toc() << std::endl;
}

/**
 * @brief Get file name list of specified pattern
 * @param path File path
 * @param filenames Output file name list
 * @param pattern File pattern, default is ".png"
 */
void GetFileNamesEx(std::string path, std::vector<std::string> &filenames, std::string pattern = ".png") {
	vector<cv::String> fn;
	cv::glob(path + pattern, fn, false);
	size_t count = fn.size(); // Number of PNG files in the image folder
	filenames.clear();
	for (size_t i = 0; i < count; i++) {
		filenames.emplace_back(fn[i]);
	}
}

/**
 * @brief Extract timestamp from filename
 * @param path File path
 * @param filename Filename
 * @return Timestamp string
 */
std::string GetJpgTimeEx(std::string path, std::string filename) {
	// std::vector<std::string> vecTemp;
	// vecTemp.clear();
	std::string cutFileName = filename.substr(path.length());
	// SplitString(cutFileName.substr(0, cutFileName.length() - 4), vecTemp, "_");
	// if (vecTemp[1] == strType) {
	//     return vecTemp[2];
	// } else {
	//     return "";
	// }

	return cutFileName.substr(0, cutFileName.length() - 4);
}

// Global variable declarations
std::vector<CarPose> car_pose_vec_;			   // Vehicle pose vector
std::vector<Eigen::Vector2f> heights_min_max_; // Height min-max value vector

/**
 * @brief Load vehicle pose data from TXT file
 * @param path File path
 * @param car_pose Vehicle pose data mapping table
 */
void GetCarPoseFromTXT(std::string &path, std::map<int64_t, CarPose> &car_pose) {
	std::ifstream infile;
	infile.open(path); // Open file
	if (!infile) {
		std::cerr << "Cannot open file! " << path << std::endl;
		return;
	}
	char line[100];
	car_pose_vec_.clear();
	while (infile.getline(line, 100, '\n')) {
		std::vector<float> vecs;
		int64_t timestamp = 0;
		int count = 0;
		char *ptr;
		char *p;
		ptr = strtok_r(line, " ,", &p);
		while (ptr) {
			// Data parsing logic
			// if (count == 3) {
			//     timestamp = std::atoll(ptr);
			// } else {
			//     float f = std::atof(ptr);
			//     vecs.push_back(f);
			//     // std::cout << "f " << f;
			// }

			// count++;
			// if (count == 7) {
			//     break;
			// }
			if (g_mapping_mode == 1) {
				if (count == 6) {
					timestamp = std::atoll(ptr);
				}
				else {
					float f = std::atof(ptr);
					vecs.push_back(f);
					// std::cout << "f " << f;
				}

				count++;
				if (count == 7) {
					break;
				}
			}
			else {
				if (count == 6) {
					timestamp = std::atoll(ptr);
				}
				else {
					float f = std::atof(ptr);
					vecs.push_back(f);
					// std::cout << "f " << f;
				}

				count++;
				if (count == 7) {
					break;
				}
			}

			ptr = strtok_r(NULL, " ,", &p);
		}
		std::cout << std::endl;
		// return;

		// Vehicle pose data structure description:
		// float x;
		// float y;
		// float z;
		// float yaw;
		// float pitch;
		// float roll;

		// CarPose data_point(vecs[0], vecs[1], vecs[2], timestamp);
		// car_pose_out << car_pose.x << "," << car_pose.y << "," << car_pose.yaw << "," << car_pose.timestamp
		// << "," << car_pose.pitch << "," << car_pose.roll << "," << car_pose.z << ","
		// << global_map.car_pose_vec[i].first << "\n";
		// 0-x 1-y 2-yaw 3-timestamp 4-pitch 5-roll 6-z

		//  CarPose(float a, float b, float c, float d, float e, float f, int64_t t)
		// : x(a), y(b), z(c), yaw(d), pitch(e), roll(f), timestamp(t) {}
		// x,y,z,yaw,pitch,roll,class,time
		// CarPose data_point(vecs[0], vecs[1], vecs[5], vecs[2], vecs[3], vecs[4], timestamp);
		CarPose data_point(vecs[0], vecs[1], vecs[2], vecs[3], vecs[4], vecs[5], timestamp);
		car_pose[timestamp] = data_point;
		// std::cout << "timestamp= " << timestamp << std::endl;
		car_pose_vec_.push_back(data_point);
		vecs.clear();
	}
	infile.close(); // Close file
}

/**
 * @brief Load point cloud data from TXT file
 * @param cloud_path Point cloud file path
 * @param cloud_txt_map_load Loaded point cloud data
 * @return true on success, false on failure
 */
bool loadTxtToPcl(std::string cloud_path, pcl::PointCloud<pcl::PointXYZI> &cloud_txt_map_load) {
	// Read point cloud data
	// std::vector<Eigen::Vector3f> vecMap;
	// pcl::PointCloud<pcl::PointXYZI>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZI>);
	cloud_txt_map_load.clear();
	std::ifstream infile;
	infile.open(cloud_path);
	if (!infile) {
		// LOG_INFO("[VLOCATION] ReadCloud cannot open cloud file : {}", cloud_path);
		return false;
	}
	char line[100];
	while (infile.getline(line, 100, '\n')) {
		if (line[0] == '#') {
			continue;
		}
		std::vector<float> vecs;
		int count = 0;
		char *ptr;
		char *p;
		ptr = strtok_r(line, " ,", &p);
		while (ptr) {
			float f = std::atof(ptr);
			vecs.push_back(f);
			count++;
			if (count == 4) {
				break;
			}
			ptr = strtok_r(NULL, " ,", &p);
		}
		// tmp.add(Eigen::Vector2f(vecs[1], -vecs[0]));
		// vecMap.push_back(Eigen::Vector3f(vecs[0], vecs[1], vecs[2]));
		pcl::PointXYZI tmp_point;
		if (count == 4) {
			tmp_point.x = vecs[0];
			tmp_point.y = vecs[1];
			tmp_point.z = vecs[2];
			tmp_point.intensity = vecs[3];
		}
		else if (count == 3) {
			tmp_point.x = vecs[0];
			tmp_point.y = vecs[1];
			tmp_point.z = vecs[2];
		}
		else if (count == 2) {
			tmp_point.x = vecs[0];
			tmp_point.y = vecs[1];
		}

		cloud_txt_map_load.push_back(tmp_point);
		vecs.clear();
	}
	infile.close(); // Close file
	return true;
}

/**
 * @brief Convert PCD (Point Cloud Data) file to TXT file
 * @param pcd_path PCD file path
 * @param text_path Output TXT file path
 */
void pcd_to_txt(std::string pcd_path, std::string text_path) {
	pcl::PointCloud<pcl::PointXYZI>::Ptr load_map(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::io::loadPCDFile(pcd_path.c_str(), *load_map);
	std::fstream fout;
	fout.open(text_path.c_str(), std::ios::out);
	for (size_t i = 0; i < load_map->size(); i++) {
		if (fout) {
			fout << load_map->points[i].x << "," << load_map->points[i].y << "," << load_map->points[i].z << ","
				 << load_map->points[i].intensity << std::endl;
		}
	}
	if (fout) {
		fout.close();
	}
}

/**
 * @brief Calculate median of float vector
 * @param values Float vector
 * @return Median value
 */
float median(std::vector<float> values) {
	// Return the median element of the float vector
	nth_element(values.begin(), values.begin() + values.size() / 2, values.end());
	return values[values.size() / 2];
};

/**
 * @brief Get multi-layer Z height information
 * @param car_pose_vec Vehicle pose vector
 */
void get_multi_z(std::vector<CarPose> car_pose_vec) {
	// float fLastAngle = 0;
	std::vector<std::vector<float>> heights;
	std::vector<float> height;
	height.clear();
	heights.clear();
	std::cout << "car_pose_vec.size() " << car_pose_vec.size() << std::endl;
	if (car_pose_vec.size() < 3) {
		return;
	}
	float lastMeanZ = 100.0;
	for (size_t i = 0; i < car_pose_vec.size() - 2; i++) {
		float z1 = car_pose_vec[i].z;
		float z2 = car_pose_vec[i + 1].z;
		float z3 = car_pose_vec[i + 2].z;

		float y1 = car_pose_vec[i].y;
		float y2 = car_pose_vec[i + 1].y;
		float y3 = car_pose_vec[i + 2].y;

		float x1 = car_pose_vec[i].x;
		float x2 = car_pose_vec[i + 1].x;
		float x3 = car_pose_vec[i + 2].x;

		float diffX = x2 - x1;
		float diffY = y2 - y1;
		float diffZ = z2 - z1;

		float angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI;

		float diffX1 = x3 - x2;
		float diffY1 = y3 - y2;
		float diffZ1 = z3 - z2;

		float next_angle = atan2(diffZ1, sqrt(diffX1 * diffX1 + diffY1 * diffY1)) * 180 / M_PI;

		// Debug output
		// std::cout << "z1 " << z1 << " z2 " << z2 << std::endl;
		// std::cout << "x1 " << x1 << " x2 " << x2 << std::endl;
		// if (z1 > 10) {
		//     std::cout << "z1 > 10 angle " << angle << " diffZ " << diffZ << std::endl;
		// }
		if ((fabs(next_angle) < 3 || fabs(angle) < 3) && fabs(diffZ) < 0.5 && fabs(z1 - lastMeanZ) > 1.0) {
			// Less than 3 degrees, considered as ground points
			height.push_back(z1);
			// height.push_back(z2);
		}
		else {
			if (height.size() > 5) {
				// int heightIndex = std::min_element(height.begin(), height.end()) - height.begin();
				// lastMeanZ = height[heightIndex];
				lastMeanZ = median(height);
				std::cout << "angle " << angle << " next_angle " << next_angle << " diffZ " << diffZ << std::endl;
				std::cout << "+++++++++++++z1: " << z1 << " z2 " << z2 << " lastMeanZ " << lastMeanZ << std::endl;
				heights.push_back(height);
			}
			height.clear();
		}
		// fLastAngle = angle;
	}
	if (height.size() > 5) {
		// Process end data
		heights.push_back(height);
		height.clear();
	}

	std::cout << "heights.size() " << heights.size() << std::endl;
	for (size_t i = 0; i < heights.size(); i++) {
		if (heights[i].size() > 0) {
			int min_index = std::min_element(heights[i].begin(), heights[i].end()) - heights[i].begin();
			int max_index = std::max_element(heights[i].begin(), heights[i].end()) - heights[i].begin();

			Eigen::Vector2f height_min_max(heights[i][min_index], heights[i][max_index]);
			heights_min_max_.push_back(height_min_max);
			std::cout << "height min " << heights[i][min_index] << " " << heights[i][max_index] << std::endl;
		}
	}
	heights.clear();
}

/**
 * @brief Set multi-layer Z height information
 * @param car_poses_loc Vehicle pose data mapping table
 */
void set_multi_z(std::map<int64_t, CarPose> &car_poses_loc) {
	// Use iterator to traverse the map
	for (auto it = car_poses_loc.begin(); it != car_poses_loc.end(); ++it) {
		// std::cout << "Key: " << it->first << ", Value: " << it->second << std::endl;
		for (size_t i = 0; i < heights_min_max_.size(); i++) {
			if (it->second.z >= heights_min_max_[i][0] - 0.25 && it->second.z <= heights_min_max_[i][1] + 0.25) {
				it->second.z = 0.5 * (heights_min_max_[i][1] + heights_min_max_[i][1]);
				it->second.roll = 0;
				it->second.pitch = 0;
			}
		}
		// std::cout << "it->second.z " << it->second.z << std::endl;
	}
}

// Visualization process function (commented out)
// void process() {
//     while (!viewer.wasStopped()) {
//         viewer.spinOnce(100);
//         // viewer->spinOnce(100);
//         // boost::this_thread::sleep(boost::posix_time::microseconds(1000));
//         // usleep(1000 * 10);
//     }
// }

/**
 * @brief Get split string
 * @param s Input string
 * @param c Delimiter
 * @return Split string
 */
std::string getSplitString(const std::string &s, const std::string &c) {
	std::string::size_type pos1, pos2;
	pos2 = s.rfind(c);
	pos1 = 0;
	return s.substr(pos1, pos2 - pos1);
}

/**
 * @brief Set terminal to non-blocking mode
 */
void setNonBlockingTerminal() {
	struct termios ttystate;
	tcgetattr(STDIN_FILENO, &ttystate);
	// Set the ICANON flag bit in the c_lflag member of the ttystate structure to 0. ICANON is a macro definition indicating the terminal's canonical mode.
	// By setting the ICANON flag bit to 0, the terminal can be set to non-canonical mode. In non-canonical mode, the terminal will immediately send input characters to the program without waiting for the user to press Enter.
	ttystate.c_lflag &= ~ICANON;
	// Clear the ECHO bit in the c_lflag field to turn off the terminal's echo function.
	// ttystate.c_lflag &= ~ECHO;
	ttystate.c_cc[VMIN] = 0;
	ttystate.c_cc[VTIME] = 0;
	tcsetattr(STDIN_FILENO, TCSANOW, &ttystate);
}

/**
 * @brief Recursively create directory
 * @param pDirName Directory path
 * @return 0 on success, -1 on failure
 */
int createDirectory(const char *pDirName) {
	constexpr int kMaxLength = 1024;
	int dirPathLen = strlen(pDirName);
	if (dirPathLen > kMaxLength) {
		return -1;
	}

	char tmpDirPath[kMaxLength] = { 0 };
	for (int i = 0; i < dirPathLen; ++i) {
		tmpDirPath[i] = pDirName[i];
		if (tmpDirPath[i] == '\\' || tmpDirPath[i] == '/') {
			if (access(tmpDirPath, 0) != 0) {
				// mkdir(path,S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)
				int ret = mkdir(tmpDirPath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
				if (ret != 0) {
					return ret;
				}
			}
		}
	}
	return 0;
}

/**
 * @brief Main function
 */
int main() {
	// Set terminal to non-blocking mode
	setNonBlockingTerminal();

	std::deque<std::pair<int64_t, std::string>> file_time_names_loc;
	std::map<int64_t, CarPose> car_poses_loc;
	pcl::PointCloud<pcl::PointXYZI>::Ptr single_map_cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr all_map_cloud(new pcl::PointCloud<pcl::PointXYZI>);

	// Read configuration file
	std::string seg_path_loc = getCmdResult("cat config/jpg_map.txt | tail -n +1 | head -n 1");
	if (seg_path_loc[seg_path_loc.length() - 1] != '/') {
		seg_path_loc += "/";
	}
	std::string car_pose_loc_path = getCmdResult("cat config/jpg_map.txt | tail -n +2 | head -n 1");
	std::string str_save_rot_pcd = getCmdResult("cat config/jpg_map.txt | tail -n +3 | head -n 1");
	std::string str_save_01_yuyi = getCmdResult("cat config/jpg_map.txt | tail -n +4 | head -n 1");
	std::string str_mapping = getCmdResult("cat config/jpg_map.txt | tail -n +5 | head -n 1");

	std::cout << "---------------1----------------" << std::endl;

	// Read coordinate point information
	std::string strPoint1 =
		getCmdResult("cat utm_test/3/apa_car_output.txt | tail -n +1 | head -n 1");
	std::string strPoint2 =
		getCmdResult("cat utm_test/3/apa_car_output.txt | tail -n +2 | head -n 1");
	std::string strPoint3 =
		getCmdResult("cat utm_test/3/apa_car_output.txt | tail -n +3 | head -n 1");
	std::string strPoint4 =
		getCmdResult("cat utm_test/3/apa_car_output.txt | tail -n +4 | head -n 1");

	std::cout << "---------------2----------------" << std::endl;
	// Set global parameters
	g_save_rotated_pcd = std::atoi(str_save_rot_pcd.c_str());
	g_save_semantic_01 = std::atoi(str_save_01_yuyi.c_str());
	g_mapping_mode = std::atoi(str_mapping.c_str());

	// Determine map file path
	std::string str_map_path = getSplitString(car_pose_loc_path, "/");
	// std::string str_map_path_MPA = str_map_path + "/Map.txt";
	std::string str_map_path_MPA_pre = getSplitString(str_map_path, "/");
	std::string str_map_path_MPA = str_map_path_MPA_pre + "/Map.txt";

	std::cout << "str_map_path_MPA " << str_map_path_MPA << std::endl;
	// std::string str_map_path_APA =  str_map_path + "/APAMap.txt";

	if (IsFileExist(str_map_path_MPA.c_str()) == true) {
		str_map_path = str_map_path_MPA;
	}
	else {
		str_map_path += "/APAMap.txt";
	}

	std::cout << "str_map_path " << str_map_path << std::endl;

	// Load global map
	pcl::PointCloud<pcl::PointXYZI>::Ptr global_map(new pcl::PointCloud<pcl::PointXYZI>);
	// pcl::io::loadPCDFile(str_map_path.c_str(), *global_map);
	loadTxtToPcl(str_map_path.c_str(), *global_map);

	// Get image file list
	std::vector<std::string> filenames_map;
	GetFileNamesEx(seg_path_loc, filenames_map, "*.png");

	// Get vehicle pose data
	GetCarPoseFromTXT(car_pose_loc_path, car_poses_loc);

	// Height processing (commented out)
	// get_multi_z(car_pose_vec_);
	// set_multi_z(car_poses_loc);
	// return 0;

	std::cout << "---------------4----------------" << std::endl;
	// Point cloud visualization initialization
	pcl::PointXYZI min_;
	pcl::PointXYZI max_;
	pcl::getMinMax3D(*global_map, min_, max_);
	// viewer.setCameraFieldOfView(0.785398);  // fov 45°  Field of view
	// viewer.setBackgroundColor(0.2, 0.2, 0.2);
	// viewer.setCameraPosition(max_.x + 0, max_.y, 80, 0, 0, -1, 0, 0, 0);

	// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> incloudHandler(all_map_cloud, 255, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> incloudHandler(global_map, 255, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> outcloudHandler(single_map_cloud, 255, 0, 0);
	// viewer.addPointCloud(all_map_cloud, incloudHandler, "In");
	viewer.addPointCloud(global_map, incloudHandler, "In");
	viewer.addPointCloud(single_map_cloud, outcloudHandler, "Out");

	Eigen::Matrix4d odom_guess_last = Eigen::Matrix4d::Identity();
	pcl::PointXYZ pointLast;
	pcl::PointXYZ pointNow;
	int iFirstScan = 0;

	TicToc timeTTAll;

	// sleep(2);

	// Main processing loop
	// for (auto& file : filenames_map) {
	int line_numeber = 0;
	char str_line[25]; // Assign unique ID for each line segment to avoid drawing only one line
	char str_sphere[25];

	std::cout << "---------------5----------------" << std::endl;
	for (size_t i = 0; i < filenames_map.size(); i++) {
		// Collect keyboard input
		char input;
		// bool is_paused = false;
		while (true) {
			// Keyboard control logic (commented out)
			// int key = cv::waitKey(100) & 0xff;
			// if (key == 32) {  // Space key
			//     is_paused = !is_paused;
			//     std::cout << "key " << key << std::endl;
			// }
			// if (is_paused) {
			//     continue;
			// } else {
			//     break;
			// }

			if (read(STDIN_FILENO, &input, 1) != -1) {
				if (is_paused == false && input == ' ') {
					is_paused = true;
				}
				else if (is_paused == true && input == ' ') {
					is_paused = false;
					// break;
				}
			}
			input = 'w';

			if (is_paused) {
				// std::cout << "Paused" << std::endl;
				viewer.spinOnce(100);
				continue;
			}
			else {
				// std::cout << "Resumed" << std::endl;
				break;
			}
		}

		string time = "";

		time = GetJpgTimeEx(seg_path_loc, filenames_map[i]);

		// std::cout << "time " << time << std::endl;
		// std::cout << "file " << file << std::endl;

		int64_t timestamp = std::atoll(time.c_str());
		static int64_t last_timestamp = timestamp;
		int64_t diff_time = timestamp - last_timestamp;
		if (diff_time > 500000) {
			std::cout << "timestamp diff " << timestamp - last_timestamp << std::endl;
		}

		last_timestamp = timestamp;

		// double timestamp = std::atoll(time.c_str());
		file_time_names_loc.emplace_back(std::make_pair(timestamp, filenames_map[i]));

		g_jpg_size = file_time_names_loc.size();
		// std::cout << "timestamp " << timestamp << " file " << file << std::endl;

		// Calculate pose transformation matrix
		Eigen::Matrix4d odom_guess = Eigen::Matrix4d::Identity();
		if (g_save_rotated_pcd > 0) {
			// odom_guess =
			//     (Eigen::Translation3d(car_poses_loc[timestamp].x, car_poses_loc[timestamp].y, 0) *
			//      Eigen::AngleAxisd(car_poses_loc[timestamp].yaw, Eigen::Vector3d::UnitZ()) *
			//      Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()))
			//         .matrix();
			// Debug output
			// std::cout << "car_poses_loc[timestamp].x " << car_poses_loc[timestamp].x << std::endl;
			// std::cout << "car_poses_loc[timestamp].y " << car_poses_loc[timestamp].y << std::endl;
			// std::cout << "car_poses_loc[timestamp].z " << car_poses_loc[timestamp].z << std::endl;
			// std::cout << "car_poses_loc[timestamp].yaw " << car_poses_loc[timestamp].yaw << std::endl;
			// std::cout << "car_poses_loc[timestamp].pitch " << car_poses_loc[timestamp].pitch << std::endl;
			// std::cout << "car_poses_loc[timestamp].roll " << car_poses_loc[timestamp].roll << std::endl;

			odom_guess = (Eigen::Translation3d(car_poses_loc[timestamp].x, car_poses_loc[timestamp].y,
							  car_poses_loc[timestamp].z) *
						  Eigen::AngleAxisd(car_poses_loc[timestamp].yaw, Eigen::Vector3d::UnitZ()) *
						  Eigen::AngleAxisd(car_poses_loc[timestamp].pitch, Eigen::Vector3d::UnitY()) *
						  Eigen::AngleAxisd(car_poses_loc[timestamp].roll, Eigen::Vector3d::UnitX()))
							 .matrix();
		}

		Eigen::Matrix4d delta_odom_guess = odom_guess_last.inverse() * odom_guess;

		Eigen::Affine3f aff_pose;

		aff_pose.matrix() = delta_odom_guess.cast<float>();
		float x, y, z, roll, pitch, yaw;
		pcl::getTranslationAndEulerAngles(aff_pose, x, y, z, roll, pitch, yaw);

		// Eigen::Vector3f pt_last(odom_guess_last(0, 3), odom_guess_last(1, 3), odom_guess_last(2, 3));
		// Eigen::Vector3f pt_now(odom_guess(0, 3), odom_guess(1, 3), odom_guess(2, 3));
		pointNow.x = odom_guess(0, 3);
		pointNow.y = odom_guess(1, 3);
		pointNow.z = odom_guess(2, 3);

		pointLast.x = odom_guess_last(0, 3);
		pointLast.y = odom_guess_last(1, 3);
		pointLast.z = odom_guess_last(2, 3);

		bool bWhole = false;
		if (i == 0 || i == filenames_map.size() - 1) {
			bWhole = true;
		}

		// Image to point cloud data processing
		jpg_to_pcl(filenames_map[i], single_map_cloud, odom_guess, bWhole);
		*all_map_cloud += *single_map_cloud;

		// Range restriction (commented out)
		// if (pointNow.x > 10) {
		//     // Only store starting point cloud
		//     break;
		// }

		if (iFirstScan == 0) {
			pointLast = pointNow;
		}
		iFirstScan = 1;

		char str_line1[25];
		char str_line2[25];

		if (single_map_cloud->size() > 0) {
			// Update visualization
			// viewer.updatePointCloud(all_map_cloud, incloudHandler, "In");
			viewer.updatePointCloud(global_map, incloudHandler, "In");
			viewer.removePointCloud("Out");
			viewer.addPointCloud(single_map_cloud, outcloudHandler, "Out");
			// viewer.updatePointCloud(single_map_cloud, outcloudHandler, "Out");

			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, str_line);
			viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, str_line);
			line_numeber++;						   // Increment sequentially to achieve different ID numbers
			sprintf(str_line, "%d", line_numeber); // Convert number to string, required for addLine, addLine function defined below
			viewer.addLine<pcl::PointXYZ>(pointLast, pointNow, str_line);

			// Add sphere, parameters are: center point, radius, r, g, b
			sprintf(str_sphere, "sphere_%d", line_numeber);
			viewer.addSphere(pointNow, 0.2, 0.0, 0.0, 1.0, str_sphere);

			// Add key point spheres
			pcl::PointXYZ point1;
			point1.x = point1_x_new;
			point1.y = point1_y_new;
			point1.z = 0;
			viewer.addSphere(point1, 0.2, 1, 1, 0, "point1");

			pcl::PointXYZ point2;
			point2.x = point2_x_new;
			point2.y = point2_y_new;
			point2.z = 0;
			viewer.addSphere(point2, 0.2, 1, 1, 0, "point2");

			pcl::PointXYZ point3;
			point3.x = point3_x_new;
			point3.y = point3_y_new;
			point3.z = 0;
			viewer.addSphere(point3, 0.2, 1, 1, 0, "point3");

			pcl::PointXYZ point4;
			point4.x = point4_x_new;
			point4.y = point4_y_new;
			point4.z = 0;
			viewer.addSphere(point4, 0.2, 1, 1, 0, "point4");

			// Visualize first fitted line (commented out)
			// if (coefficients1.size() == 6) {
			//     sprintf(str_line1, "line1_%d", line_numeber);
			//     viewer.addLine<pcl::PointXYZ>(
			//         pcl::PointXYZ(coefficients1[0] - 10 * coefficients1[3], coefficients1[1] - 10 * coefficients1[4],
			//                       coefficients1[2] - 10 * coefficients1[5]),
			//         pcl::PointXYZ(coefficients1[0] + 10 * coefficients1[3], coefficients1[1] + 10 * coefficients1[4],
			//                       coefficients1[2] + 10 * coefficients1[5]),
			//         0, 0, 1, str_line1);
			// }

			// Visualize second fitted line (commented out)
			// if (coefficients2.size() == 6) {
			//     sprintf(str_line2, "line2_%d", line_numeber);
			//     viewer.addLine<pcl::PointXYZ>(
			//         pcl::PointXYZ(coefficients2[0] - 10 * coefficients2[3], coefficients2[1] - 10 * coefficients2[4],
			//                       coefficients2[2] - 10 * coefficients2[5]),
			//         pcl::PointXYZ(coefficients2[0] + 10 * coefficients2[3], coefficients2[1] + 10 * coefficients2[4],
			//                       coefficients2[2] + 10 * coefficients2[5]),
			//         1, 0, 0, str_line2);
			// }
		}

		odom_guess_last = odom_guess;
		pointLast = pointNow;

		viewer.spinOnce(50);
		if (is_paused == false) {
			viewer.removeShape(str_line1);
			viewer.removeShape(str_line2);
			viewer.removeShape("point1");
			viewer.removeShape("point2");
			viewer.removeShape("point3");
			viewer.removeShape("point4");
		}
		// usleep(1000 * 1000);
	}

	std::cout << "for all covert cost ms " << timeTTAll.toc() << std::endl;

	// Final point cloud processing
	// pcl::PointCloud<pcl::PointXYZI>::Ptr map_only23456_filter(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter_scan;
	voxel_grid_filter_scan.setLeafSize(0.2, 0.2, 0.2);
	voxel_grid_filter_scan.setInputCloud(all_map_cloud);
	voxel_grid_filter_scan.filter(*all_map_cloud);

	// Save final results
	pcl::io::savePCDFileBinary("test_data/map_only23456.pcd", *all_map_cloud);
	pcd_to_txt("test_data/map_only23456.pcd", "test_data/map_only23456.txt");
	std::cout << "file_time_names_loc size() " << file_time_names_loc.size() << std::endl;
	std::cout << "car_poses_loc size() " << car_poses_loc.size() << std::endl;
}
